#ifndef QNODE_H
#define QNODE_H

#ifndef Q_MOC_RUN
#include <ros/ros.h>
#endif

#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <move_base_msgs/MoveBaseGoal.h>
#include <ros/network.h>
#include <ros/ros.h>
#include <ros_qt_msgs/AdjustSpeed.h>
#include <ros_qt_msgs/Msg.h>
#include <ros_qt_msgs/NavStatus.h>
#include <ros_qt_msgs/RefReset.h>
#include <ros_qt_msgs/Relocation.h>
#include <ros_qt_msgs/SetMoveMode.h>
#include <ros_qt_msgs/TargetPose.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_srvs/SetBool.h>
#include <tf/transform_datatypes.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <QDebug>
#include <QThread>
#include <QTimer>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <station.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int8.h>

#define coordinateConversionCoefficient 1000.0
#define speedConversionCoefficient 100.0

namespace qnode {

typedef actionlib::SimpleActionClient< move_base_msgs::MoveBaseAction > MoveBaseClient;

class Publishers {
public:
    ros::NodeHandle nh{ "ros_qt" };

    ros::Publisher     currentSpeedPub{ nh.advertise< std_msgs::Float64MultiArray >( "current_speed", 1 ) };
    ros::Publisher     initialPosePub{ nh.advertise< geometry_msgs::PoseWithCovarianceStamped >( "/initialpose", 1 ) };
    ros::ServiceClient forceSlowClient{ nh.serviceClient< std_srvs::SetBool >( "force_slow" ) };
    ros::ServiceClient lockLocationClient{ nh.serviceClient< std_srvs::SetBool >( "/can_initial_pose" ) };
    ros::ServiceClient reflectiveLocalizationEnableClient{ nh.serviceClient< std_srvs::SetBool >( "/reflective_localization_enable" ) };
    ros::ServiceClient reflectiveLocalizationResetClient{ nh.serviceClient< ros_qt_msgs::RefReset >( "/reflective_localization_reset" ) };

    ros::Publisher startInitPosePub{ nh.advertise< std_msgs::Bool >( "/startinitpose", 1 ) };
    ros::Publisher navGoalMarkerPub{ nh.advertise< geometry_msgs::PoseArray >( "/navgoal", 1 ) };
};

class QNode : public QThread {
    Q_OBJECT

Q_SIGNALS:
    void sigSendTips( const QString& );
    void sigRosShutdown();  //正常运行时候发生严重错误，再调用发生给qt
    void sigcurrentPose( const QList< double >& );
    void sigcurrentBoardPose( const QList< double >& );
    void sigtargetSpeed( const QList< double >& );
    void sigonGoalReached();
    void sigonNavStatus( const int& );
    void sigonReflectiveStatus( const int& );
    void sigonLocalizationLose();
    void sigonRelocalizationResult( const int& );

private:
    int                                             init_argc;
    char**                                          init_argv;
    bool                                            is_rosmaster_run = false;
    boost::shared_ptr< MoveBaseClient >             client;
    boost::shared_ptr< Publishers >                 publishers;
    boost::shared_ptr< tf2_ros::Buffer >            tf_buffer;
    boost::shared_ptr< tf2_ros::TransformListener > tf_listener;

    ros::Timer map_timer;
    ros::Timer board_timer;

    void run() {
        int code = main();
        if ( code < 0 ) { Q_EMIT sigRosShutdown(); }
    }

public:
    QNode( int argc, char** argv ) : init_argc( argc ), init_argv( argv ) {}

    ~QNode() {
        if ( ros::isStarted() ) {
            Q_EMIT sigRosShutdown();
            ros::shutdown();
            ros::waitForShutdown();
        }
        wait();
    }

    int main() {
        ros::init( init_argc, init_argv, "qnode_qt" );
        if ( !ros::master::check() ) {
            Q_EMIT sigSendTips( QString( "错误：ros master 可能未运行" ) );
            return -1;
        }
        ros::start();
        ros::NodeHandle nh( "ros_qt" );

        publishers = boost::make_shared< Publishers >();

        ros::Subscriber sub                 = nh.subscribe< std_msgs::Float64MultiArray >( "target_speed", 1, &QNode::targetSpeedSub_CB, this );
        ros::Subscriber sub5                = nh.subscribe< ros_qt_msgs::NavStatus >( "/move_base/nav_status", 1, &QNode::onNavStatus, this );
        ros::Subscriber sub6                = nh.subscribe< std_msgs::Empty >( "/localization_guarder", 1, &QNode::onLocalizationLose, this );
        ros::Subscriber relocationResultSub = nh.subscribe< std_msgs::Int8 >( "/initposeresult", 1, &QNode::onRelocalizationResult, this );
        ros::Subscriber sub7                = nh.subscribe< ros_qt_msgs::NavStatus >( "/reflective_localization/nav_status", 1, &QNode::getReflectiveStatus, this );
        ros::Subscriber sub8                = nh.subscribe< std_msgs::Float64MultiArray >( "current_pose/board", 1, &QNode::currentBoardPoseTimer_CB, this );

        if ( !publishers->lockLocationClient.exists() ) { Q_EMIT sigSendTips( QString( "错误：ros-定位锁定服务未开启" ) ); }
        if ( !publishers->forceSlowClient.exists() ) { Q_EMIT sigSendTips( QString( "错误：ros-强制慢速服务未开启" ) ); }

        client = boost::make_shared< MoveBaseClient >( "move_base", true );

        tf_buffer = boost::make_shared< tf2_ros::Buffer >( ros::Duration( 5 ) );
        tf_buffer->setUsingDedicatedThread( true );
        tf_listener = boost::make_shared< tf2_ros::TransformListener >( *tf_buffer );

        map_timer = nh.createTimer( ros::Duration( 0.1 ), &QNode::currentMapPoseTimer_CB, this, false, true );

        // if (!publishers->reflectiveLocalizationEnableClient.exists()) { Q_EMIT sigSendTips(QString("错误：ros-反光板定位服务未开启")); }
        // if (!publishers->reflectiveLocalizationResetClient.exists()) { Q_EMIT sigSendTips(QString("错误：ros-反光板重置服务未开启")); }
        //  board_timer = nh.createTimer(ros::Duration(0.1), &QNode::currentBoardPoseTimer_CB, this, false, false);

        Q_EMIT sigSendTips( QString( "提示：ros qnode 初始化完成" ) );
        is_rosmaster_run = true;
        ros::spin();
        return 0;
    }

    ////////////////////////////一些回调

    /**
     * @brief 导航行进状态回调（status.type<0 时无法导航，==0 时正常，>0 时还有路走但不能到终点） status.msg <- 这是对应文本消息
     * @param status
     */
    void onNavStatus( const ros_qt_msgs::NavStatus status ) {
        Q_EMIT sigonNavStatus( static_cast< int >( status.type ) );
    }

    /**
     * @brief 定位丢失
     */
    void onLocalizationLose( const std_msgs::Empty ) {
        Q_EMIT sigonLocalizationLose();
    }

    void move_base_CB( const actionlib::SimpleClientGoalState& state, const boost::shared_ptr< const move_base_msgs::MoveBaseResult >& ) {
        if ( state.isDone() ) {
            switch ( state.state_ ) {
            case actionlib::SimpleClientGoalState::SUCCEEDED:
                Q_EMIT sigonGoalReached();
                break;
            case actionlib::SimpleClientGoalState::PREEMPTED:
                Q_EMIT sigSendTips( QString( "SLAM导航——%1" ).arg( state.getText().c_str() ) );
                break;
            case actionlib::SimpleClientGoalState::PENDING:
            case actionlib::SimpleClientGoalState::ACTIVE:
            case actionlib::SimpleClientGoalState::RECALLED:
            case actionlib::SimpleClientGoalState::REJECTED:
            case actionlib::SimpleClientGoalState::ABORTED:
            case actionlib::SimpleClientGoalState::LOST:
                Q_EMIT sigSendTips( QString( "错误：SLAM导航异常——%1" ).arg( state.getText().c_str() ) );
                break;
            }
        }
    }

    /**
     * @brief 当前位姿回调，订阅参数单位m
     * @param pose
     */
    void currentMapPoseTimer_CB( const ros::TimerEvent& ) {
        static unsigned char tips_flag = -1;
        try {
            geometry_msgs::TransformStamped tran = tf_buffer->lookupTransform( "map", "base_footprint", ros::Time( 0 ), ros::Duration( 1 ) );
            QList< double >                 poselist;
            poselist.append( tran.transform.translation.x * coordinateConversionCoefficient );  // x
            poselist.append( tran.transform.translation.y * coordinateConversionCoefficient );  // y
            poselist.append( tf::getYaw( tran.transform.rotation ) );                           // th
            Q_EMIT sigcurrentPose( poselist );
            if ( tips_flag != 1 ) {
                tips_flag = 1;
                Q_EMIT sigSendTips( QString( "获取SLAM定位成功[map]" ) );
            }
        }
        catch ( std::runtime_error& ex ) {
            if ( tips_flag != 0 ) {
                tips_flag = 0;
                Q_EMIT sigSendTips( QString( "错误：获取SLAM定位异常[map]——%1" ).arg( ex.what() ) );
            }
        }
    }

    /**
     * @brief 订阅导航速度，单位m/s
     * @param motor
     */
    void targetSpeedSub_CB( const std_msgs::Float64MultiArray motor ) {
        QList< double > speed;
        speed.append( motor.data[ 0 ] * speedConversionCoefficient );  // left
        speed.append( motor.data[ 1 ] * speedConversionCoefficient );  // right
        Q_EMIT sigtargetSpeed( speed );
        // Q_EMIT sigSendTips( QString( "l:%1 r:%2" ).arg( motor.data[ 0 ] ).arg(
        // motor.data[ 1 ] ) );
    }

    ////////////////////////////可调用

    /**
     * @brief 使能反光板定位，返回是否开启成功
     */
    bool enableReflectiveLocalization( bool enable ) {
        if ( !is_rosmaster_run ) { return false; }

        if ( enable ) { board_timer.start(); }
        else {
            board_timer.stop();
        }

        if ( publishers->reflectiveLocalizationEnableClient.exists() ) {
            std_srvs::SetBool data;
            data.request.data = enable;
            return publishers->reflectiveLocalizationEnableClient.call( data ) && data.response.success;
        }
        else {
            Q_EMIT sigSendTips( QString( "错误：ros reflectiveLocalizationEnableClient服务不存在！" ) );
            return false;
        }
    }

    /**
     * @brief 重置反光板参考全局坐标，且使能反光板定位
     */
    bool reflectiveReset( double x, double y ) {
        if ( !is_rosmaster_run ) { return false; }

        if ( publishers->reflectiveLocalizationResetClient.exists() ) {
            ros_qt_msgs::RefReset data;
            data.request.use_global_position = true;
            data.request.x                   = x;
            data.request.y                   = y;
            return publishers->reflectiveLocalizationResetClient.call( data ) && data.response.success;
        }
        else {
            Q_EMIT sigSendTips( QString( "错误：ros reflectiveLocalizationResetClient服务不存在！" ) );
            return false;
        }
    }

    /**
     * @brief AMCL重定位
     * @param x
     * @param y
     * @param yaw
     * @return
     */
    bool relocation( double x, double y, double yaw ) {
        if ( !is_rosmaster_run ) { return false; }

        geometry_msgs::PoseWithCovarianceStamped msg;
        msg.header.frame_id       = "map";
        msg.header.stamp          = ros::Time::now();
        msg.pose.pose.position.x  = x;
        msg.pose.pose.position.y  = y;
        msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw( yaw );
        msg.pose.covariance       = { 0.25, 0.0,  0.0, 0.0, 0.0, 0.0,  //
                                0.0,  0.25, 0.0, 0.0, 0.0, 0.0,        //
                                0.0,  0.0,  0.0, 0.0, 0.0, 0.0,        //
                                0.0,  0.0,  0.0, 0.0, 0.0, 0.0,        //
                                0.0,  0.0,  0.0, 0.0, 0.0, 0.0,        //
                                0.0,  0.0,  0.0, 0.0, 0.0, 0.06853891945200942 };
        publishers->initialPosePub.publish( msg );
        return true;
    }

    /**
     *@brief 自动重定位
     *@param
     *@return
     */
    void startRelocalization() {
        std_msgs::Bool msg;
        msg.data = true;
        publishers->startInitPosePub.publish( msg );
    }
    /**
     *@brief 自动重定位结果
     *@param
     *@return
     */
    void onRelocalizationResult( const std_msgs::Int8 msg ) {
        Q_EMIT sigonRelocalizationResult( static_cast< int >( msg.data ) );  //收到1定位成功 收到0定位失败
    }
    /**
    * @brief 放光板识别状态回调（status.type=0 为反光板识别失败，status.type=1反光板识别成功，status.type=2反光板坐标发生偏移） status.msg <- 这是对应文本消息
    * @param status
    */
    void getReflectiveStatus( const ros_qt_msgs::NavStatus status ) {
        Q_EMIT sigonReflectiveStatus( static_cast< int >( status.type ) );
    }
    /**
     * @brief 当前位姿回调，订阅参数单位m
     * @param pose
     */
    void currentBoardPoseTimer_CB( const std_msgs::Float64MultiArray pose ) {

        QList< double > poselist;
        poselist.append( pose.data.at( 0 ) * coordinateConversionCoefficient );  // x
        poselist.append( pose.data.at( 1 ) * coordinateConversionCoefficient );  // y
        poselist.append( pose.data.at( 2 ) );                                    // th
        Q_EMIT sigcurrentBoardPose( poselist );
    }
    /**
     *@brief 接收到需要显示的slampoint数据
     *@param
     *@return
     */
public Q_SLOTS:
    void sltSendSlamPoint( const QList< QList< double > > slam_point ) {
        if ( !is_rosmaster_run ) { return; }
        if ( publishers == nullptr ) { return; }

        geometry_msgs::PoseArray nav_goal;
        foreach ( QList< double > demo, slam_point )  //
        {
            geometry_msgs::Pose pose;
            if ( demo.size() >= 3 ) {
                pose.position.x    = demo.at( 1 ) / coordinateConversionCoefficient;  // x
                pose.position.y    = demo.at( 2 ) / coordinateConversionCoefficient;  // y
                pose.position.z    = demo.at( 0 );                                    // id
                pose.orientation.w = 1.0;
            }
            nav_goal.poses.push_back( pose );
        }
        publishers->navGoalMarkerPub.publish( nav_goal );
        qDebug() << "success send slam point to rviz show! size:" << slam_point.size();
    }

    /**
     * @brief 强制慢速
     * @param slow
     * @return
     */
    bool sltSetForceSlow( bool slow ) {
        if ( !is_rosmaster_run ) { return false; }
        if ( publishers->forceSlowClient.exists() ) {
            std_srvs::SetBool data;
            data.request.data = slow;
            return publishers->forceSlowClient.call( data ) && data.response.success;
        }
        else {
            Q_EMIT sigSendTips( QString( "错误：ros setForceSlow服务不存在！" ) );
            return false;
        }
    }

    /**
     * @brief 传入参数单位 m
     * @param start
     * @param target
     * @param end_yaw
     * @param mode_value  -1.取消导航 0.缺省值 1.避障  2.停障  3.反光板对接
     * @param motion_dir
     */
    void sltSetGoal( const Station& start, const QList< Station >& targets, const double& target_yaw, const std::vector< std::pair< double, double > >& speed_limits, const int mode_value,
                     const int motion_dir ) {
        if ( !is_rosmaster_run ) { return; }

        if ( targets.empty() ) {
            Q_EMIT sigSendTips( QString( "错误：qnode: 无目标点！" ) );
            return;
        }

        std_msgs::Header header;
        header.frame_id = mode_value != 3 ? "map" : "board";

        move_base_msgs::MoveBaseGoal goal;
        goal.global.startWithSpecificPose         = true;
        goal.global.start_pose.header             = header;
        goal.global.start_pose.pose.position.x    = start.point()->x() / coordinateConversionCoefficient;
        goal.global.start_pose.pose.position.y    = start.point()->y() / coordinateConversionCoefficient;
        goal.global.start_pose.pose.position.z    = start.id();
        goal.global.start_pose.pose.orientation.w = 1;

        for ( auto& target : targets ) {
            geometry_msgs::PoseStamped g;
            g.header             = header;
            g.pose.position.x    = target.point()->x() / coordinateConversionCoefficient;
            g.pose.position.y    = target.point()->y() / coordinateConversionCoefficient;
            g.pose.position.z    = target.id();
            g.pose.orientation.w = 1;
            goal.global.target_poses.push_back( g );
        }

        goal.global.target_poses.back().pose.orientation = tf::createQuaternionMsgFromYaw( target_yaw );
        goal.global.is_auto_avoid                        = mode_value > 1 ? false : true;
        goal.local.is_dock                               = false;
        goal.local.motion_dir                            = motion_dir == -1 ? -1 : 1;

        for ( auto& speed_limit : speed_limits ) {
            ros_qt_msgs::DistAndSpeed tmp;
            tmp.dist  = speed_limit.first;
            tmp.speed = speed_limit.second;
            goal.local.speed_limit.push_back( tmp );
        }

        try {
            if ( !client->waitForServer( ros::Duration( 3 ) ) ) {
                Q_EMIT sigSendTips( QString( "错误：无法连接SLAM导航服务！" ) );
                return;
            }

            client->sendGoal( goal, boost::bind( &QNode::move_base_CB, this, _1, _2 ) );
        }
        catch ( std::runtime_error& err ) {
            Q_EMIT sigSendTips( QString( "错误：无法连接SLAM导航服务——%1" ).arg( err.what() ) );
        }
    }

    /**
     * @brief 发布轮子实际速度，单位m/s
     * @param left
     * @param right
     */
    void sltUploadSpeed( const double& left, const double& right ) {
        if ( !is_rosmaster_run ) { return; }
        std_msgs::Float64MultiArray msg;
        msg.data.resize( 2 );
        msg.data[ 0 ] = left;
        msg.data[ 1 ] = right;
        publishers->currentSpeedPub.publish( msg );
        // Q_EMIT sigSendTips(QString("l:%1 r:%2").arg(msg.data[0]).arg(msg.data[1]));
    }

    /**
     * @brief 取消导航
     */
    void sltcancel() {
        if ( !is_rosmaster_run ) { return; }

        client->cancelAllGoals();
    }

    /**
     * @brief 锁定AMCL重定位
     * @param lock
     * @return
     */
    bool sltlockLocation( bool lock ) {
        if ( !is_rosmaster_run ) { return false; }
        if ( publishers->lockLocationClient.exists() ) {
            std_srvs::SetBool data;
            data.request.data = !lock;
            return publishers->lockLocationClient.call( data ) && data.response.success;
        }
        else {
            Q_EMIT sigSendTips( QString( "错误：ros lockLocation服务不存在！" ) );
            return false;
        }
    }
};

}  // namespace qnode

#endif  // QNODE_H
